package pathfinding;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Set;
import java.util.Map.Entry;

import main.Coord;

public class Pathfinding {
//	private static DebugPF db = new DebugPF();

	public static List<Coord> find(Coord start, Coord stop, boolean[][] map) {
		Node s = new Node(start.x, start.y);
		Node e = new Node(stop.x, stop.y);
		// Steg 1: hitta noder
		Collection<Node> nodelist = getNodes(s, e, map);
		
		// Steg 2: skapa graf
		for (Node n: nodelist) {
			for (Node m: nodelist) {
				if (n.see(m, map)) {
					n.addNeigh(m);
				}
			}
		}
//		db.show(map, nodelist);
		
		// Steg 3: Dijkstra's
		dijkstras(s, nodelist);
		
		// Steg 4: Return
		List<Coord> ret = new ArrayList<Coord>();
		Node n = e;
		while (n != null) {
			ret.add(0, n);
			n = n.source;
		}
		
		return ret;
	}
	
	public static Collection<Node> getNodes(Node s, Node e, boolean[][] map2) {
		Set<Node> nodelist = new HashSet<Node>();
		nodelist.add(s);
		nodelist.add(e);

		boolean[][] map = new boolean[map2.length+2][map2[0].length+2];
		for (int i=1; i<map.length-1; ++i) {
			for (int j=1; j<map[i].length-1; ++j) {
				map[i][j] = map2[i-1][j-1];
			}
		}
		
		for (int i=1; i<map.length-1; ++i) {
			for (int j=1; j<map[i].length-1; ++j) {
				if ((!map[i][j]) && 
						((map[i-1][j-1] && !map[i-1][j] && !map[i][j-1]) || 
						(map[i-1][j+1] && !map[i-1][j] && !map[i][j+1]) || 
						(map[i+1][j-1] && !map[i+1][j] && !map[i][j-1]) || 
						(map[i+1][j+1] && !map[i+1][j] && !map[i][j+1]))) {
					Node n = new Node(i-1, j-1);
					nodelist.add(n);
				}
			}
		}
		return nodelist;
	}
	
	public static void dijkstras(Node start, Collection<Node> nodes) {
		Set<Node> nodelist = new HashSet<Node>(nodes);
		start.value=0;
		while (!nodelist.isEmpty()) {
			Node current = getLeast(nodelist);
			nodelist.remove(current);
			for (Entry<Node, Double> entry: current.neigh.entrySet()) {
				Node n = entry.getKey();
				Double val = entry.getValue();
				if (nodelist.contains(n)) {
					double nVal = current.value+val;
					if (n.value > nVal) {
						n.value = nVal;
						n.source = current;
					}
				}
			}
		}
	}
	
	private static Node getLeast(Collection<Node> nodes) {
		Node min = null;
		double least = Double.POSITIVE_INFINITY;
		for (Node n: nodes) {
			if (n.value <= least) {
				least = n.value;
				min = n;
			}
		}
		if (min == null) {
			System.out.println("null from list: "+nodes+", least: "+least);
		}
		return min;
	}
	
}